﻿using BridgeShips.AI;
using System.Collections.Generic;
using UnityEngine;
using BridgeShips.Control;
using Framework.FSM;
using BridgeShips.FSM;

namespace BridgeShips
{
    public class EntityMovementControl : IControl
    {
        public uint uID;
        LineRenderer m_lineRender;
        private BezierCurve m_bezier;
        BezierMoveEntrty m_bezierMove;
        private float m_tempTextX_1 = 800;
        private float m_tempTextX_2 = 830;
        private Vector3 m_TargetPos;
        private float m_MoveKeep = 0.0f;

        private float m_DelayChangeTime = 5.0f;
        private float m_LastTargetChangeTime;
        private float m_LastTargetNotEqualCapacityTime = 0.0f;
        //List<Vector3> vectors = new List<Vector3>();
        private NoiseFilterPointBuf m_NoisePointBuffer;
        private BuildingLineChartUI m_BuildingLineChartUI;
        private int m_index = 0;
        private int m_Capacity = 5;

        //public LinkedList<Vector3> ListHeading
        //{
        //    get
        //    {
        //        return m_ListHeading;
        //    }
        //}
        public void Init(EntityBase entity)
        {
            m_bezier = new BezierCurve();
            m_bezierMove = new BezierMoveEntrty();
            VehicleEntity shipeEntity = entity as VehicleEntity;
            if (shipeEntity != null)
            {
                uID = shipeEntity.ID;
            }
            m_Capacity = 5;
            m_NoisePointBuffer = new NoiseFilterPointBuf(m_Capacity, 1.0f, 6.0f);
            m_lineRender = new LineRenderer();
        }

        private void PushNode(Vector3 direction)
        {

        }


        public void InitDown()
        {
            m_BuildingLineChartUI = GameObject.Find("Line Chart Panel UI").GetComponentInChildren<BuildingLineChartUI>();
            if (m_BuildingLineChartUI != null)
                m_BuildingLineChartUI.FillDataBuf(uID, GameEntry.Instance.VehicleEntityManager.m_list);
        }

        public void PostInit()
        {

        }
        public void Release()
        {
            Debug.Log("MoveManagerTest Release");
        }

        public void Update(float frameTime, float realTime)
        {
            if (GameEntry.Instance == null)
                return;
            var e = GameEntry.Instance.EntityManager.Get(uID);
            var briShip = e.Get().GetComponentInParent<AIEventHandler>();
            if (briShip == null || briShip.IsLinked.Is(true))
            {
                return;
            }

            var aiShip = e.Get().GetComponentInParent<AIShip>();
            if (aiShip == null || aiShip.Settings == null)
            {
                return;
            }
            if (aiShip.Settings.Movement.MovementState == ET.AIMovementState.AIDriving)
            {
                m_LastTargetChangeTime = Time.time;
                return;
            }
            else if (aiShip.Settings.Movement.MovementState == ET.AIMovementState.WaitingIdle)
            {
                Vector3 targetPosition = ShipsDictionary.Instance.GetPoisition(e.GetID());
                Vector3 shipPosition = aiShip.Settings.Detection.EntityTransform().position;
                float moveKeep = Vector3.Distance(targetPosition, shipPosition);
                if (Time.time - m_LastTargetChangeTime >= m_DelayChangeTime)
                {
                    aiShip.Settings.Movement.MovementState = ET.AIMovementState.Idle;
                }
                else if (Mathf.Abs(m_MoveKeep - moveKeep) > 0.5f)
                {
                    m_LastTargetChangeTime = Time.time;
                    m_MoveKeep = moveKeep;
                    m_bezierMove.GetPos(ShipsDictionary.Instance.GetPoisition(e.GetID()), m_bezier);
                    m_bezier.Move(e.Get(), m_bezierMove.Que);
                }

                return;
            }
            bool beFar = TransferOfAxes.Instance.linkedEachOtherFarfromSetPoint(e.Get().transform.position);
            if (beFar || !GameEntry.Instance.FsmManager.GetOpenAuxillaryConnection())
            {
                IFsmState fState = GameEntry.Instance.VirtualRManager.GetFsmHalfWayStateByShipID(uID);
                if (fState.ToString() == "BridgeShips.FSM.VHalfWayIdleState" || fState.ToString() == "BridgeShips.FSM.VHalfWayLinkingState")
                    return;
                m_TargetPos = ShipsDictionary.Instance.GetPoisition(e.GetID());
                Rotation(briShip.gameObject);

                Vector3 origalPos = e.Get().gameObject.transform.position;
                Debug.DrawLine(origalPos, m_TargetPos, Color.red, 10.0f);
                GenInterpolationPositionQueue(origalPos, m_TargetPos);
                m_bezier.Move(e.Get().gameObject, m_bezierMove.Que);
                if (m_bezierMove.Que.Count <= 0)
                {
                    //m_bezier.SyncTime(e.Get().gameObject);
                    aiShip.Settings.Movement.MovementState = ET.AIMovementState.Idle;
                }
                else
                {
                    aiShip.Settings.Movement.MovementState = ET.AIMovementState.GpsDriving;
                }
            }
            else if (GameEntry.Instance.FsmManager.GetOpenAuxillaryConnection())
            {

                IFsmState fsmState = GameEntry.Instance.VirtualRManager.GetFsmStateByShipID(uID);
                if (fsmState == null)
                    return;

                if (fsmState.ToString() == "BridgeShips.FSM.VIdleState")
                {

                }


                if (fsmState.ToString() == "BridgeShips.FSM.VStartJumpGpsFirstPositionState")
                {
                    if (m_NoisePointBuffer.Count > 0)
                    {
                        LinkedListNode<Vector3> current = m_NoisePointBuffer.ListPoints.First;
                        FsmStateDataBase fsdb = GameEntry.Instance.VirtualRManager.GetDataBaseByShipID(e.GetID());
                        if (fsdb == null)
                            return;

                        //m_TargetPos = ShipsDictionary.Instance.GetPoisition(e.GetID());
                        Quaternion targetQuat = ShipsDictionary.Instance.shipDic[uID].CurrentHeading;
                        e.Get().transform.SetPositionAndRotation(current.Value, targetQuat);
                        GameEntry.Instance.FsmManager.GetFsm(fsdb.virtualObj.name).ChangeState<VAddressCorrectState>(fsdb);
                    }
                    m_TargetPos = ShipsDictionary.Instance.GetPoisition(e.GetID());
                    Vector3 oriPos = e.Get().gameObject.transform.position;
                    if (oriPos.Equals(m_TargetPos) == false)
                        GenInterpolationPositionQueue(oriPos, m_TargetPos);
                    return;
                }
                else if (fsmState.ToString() == "BridgeShips.FSM.VPosCorrectState" || fsmState.ToString() == "BridgeShips.FSM.VIdleState"
                    || fsmState.ToString() == "BridgeShips.FSM.VLeaveState" || fsmState.ToString() == "BridgeShips.FSM.VLinkingState")
                {
                    m_bezierMove.Que.Clear();
                    m_TargetPos = GameEntry.Instance.VirtualRManager.GetVirtualShipPosition(uID);
                    return;
                }
                else
                {
                    m_TargetPos = ShipsDictionary.Instance.GetPoisition(e.GetID());
                    if (fsmState.ToString() != "BridgeShips.FSM.VLinkingState")
                        Rotation(briShip.gameObject);
                    // Debug.LogError("x = " + m_TargetPos.x + " y = " + m_TargetPos.y + " z = " + m_TargetPos.z );
                }
                Vector3 origalPos = e.Get().gameObject.transform.position;
                Debug.DrawLine(origalPos, m_TargetPos, Color.red, 10.0f);
                GenInterpolationPositionQueue(origalPos, m_TargetPos);
                m_bezier.Move(e.Get().gameObject, m_bezierMove.Que);
                if (m_bezierMove.Que.Count <= 0)
                {
                    //m_bezier.SyncTime(e.Get().gameObject);
                    aiShip.Settings.Movement.MovementState = ET.AIMovementState.Idle;
                }
                else
                {
                    aiShip.Settings.Movement.MovementState = ET.AIMovementState.GpsDriving;
                }
            }


        }

        public void GenInterpolationPositionQueue(Vector3 originalPos, Vector3 targetPos)
        {
            if (Mathf.Abs(originalPos.x - targetPos.x) < 0.02f && Mathf.Abs(originalPos.z - targetPos.z) < 0.02f)
            {
                //经过一定时间，滤波过后的数据如果没有 倾倒到 贝塞尔 强行，灌入数据
                RunForceInjectBezierCur(originalPos, targetPos, true);
                return;
            }
            else
            {
                if (m_NoisePointBuffer.Count < m_Capacity)
                {
                    bool addSuc = m_NoisePointBuffer.AddLast(targetPos);
                    if (!addSuc)
                    {
                        RunForceInjectBezierCur(originalPos, targetPos, false);
                    }
                    else
                        m_LastTargetChangeTime = Time.time;

                    return;
                }
            }
            ForceInjectBezierCurve(originalPos, targetPos, false);
        }

        private void RunForceInjectBezierCur(Vector3 originalPos, Vector3 targetPos, bool bForce)
        {
            if (Time.time - m_LastTargetChangeTime >= m_DelayChangeTime || bForce)
            {
                ForceInjectBezierCurve(originalPos, targetPos, bForce);
                // SaveFrashBezierPathQueue();
                m_LastTargetChangeTime = Time.time;
            }
        }

        public void ForceInjectBezierCurve(Vector3 originalPos, Vector3 targetPos, bool isForce)
        {
            if (m_NoisePointBuffer.Count <= 0)
                return;

            //if (isForce)
            //{
            //    m_NoisePointBuffer.IterateList();
            //}
            //else // 卡尔曼滤波
            //    m_NoisePointBuffer.UpdateBuf();

            /*
             *    Debug line chart graphic
            */
            ///-----------------------------------------------------------------------------
            if (m_BuildingLineChartUI != null && m_BuildingLineChartUI.isVisable() && (m_NoisePointBuffer.Count == m_Capacity))
                m_BuildingLineChartUI.FillDataBuf(uID, m_NoisePointBuffer.ListPoints);
            ///-----------------------------------------------------------------------------
            int index = 0;
            Vector3 preDir = (m_NoisePointBuffer.ListPoints.First.Value - originalPos).normalized;
            foreach (var p in m_NoisePointBuffer.ListPoints)
            {
                Vector3 dealedPos = new Vector3(p.x, 7.0f, p.z);
                if (index != 0)
                {
                    Vector3 curDir = (dealedPos - originalPos).normalized;
                }
                m_bezierMove.GetPos(dealedPos, m_bezier);
                originalPos = dealedPos;
                index++;
            }
            m_NoisePointBuffer.Clear();
        }

        public void SaveFrashBezierPathQueue()
        {
            foreach (var p in m_NoisePointBuffer.ListPoints)
            {
                Vector3 dealedPos = new Vector3(p.x, 7.0f, p.z);
                bool bClear = m_bezierMove.GetPos(dealedPos, m_bezier);

            }
            m_NoisePointBuffer.Clear();
        }
        public void Rotation(GameObject obj)
        {
            if (m_NoisePointBuffer.ListPoints.Count < 1)
                return;

            Quaternion targetQuat = ShipsDictionary.Instance.shipDic[uID].CurrentHeading;
            GameEntry.Instance.EntityManager.LerpForwardLocalRotation(obj.transform, targetQuat, 45.0f, 1.0f);

        }
    }
}

